/***********************************
 *  Author: Your_name
 *  brief: 调用kinematics中的正逆解求解构造Server端
 *  version: 0.1
 *  date: 2021/11/10
 * ********************************/

#include <ros/ros.h>
#include "./../../kinematics/Kine.h"
#include <birl_manipulator/InverseKine.h>

bool handle_function(birl_manipulator::InverseKine::Request &req,
                     birl_manipulator::InverseKine::Response &res)
{
    ROS_INFO_STREAM("ik_server Get New Request.");
    
    // 机器人的连杆长度
    static double Robot_Link_Len[6];
    Robot_Link_Len[0] = 0.20400;
    Robot_Link_Len[1] = 0.13670;
    Robot_Link_Len[2] = 0.29320;
    Robot_Link_Len[3] = 0.29320;
    Robot_Link_Len[4] = 0.13670;
    Robot_Link_Len[5] = 0.20400;

    // 机器人基础设置
    static Kine_IR_FiveDoF manipulator;
    manipulator.Set_Length(Robot_Link_Len);
    static double new_decartes_point[6] = {0,0,0,0,0,0};
    static double current_joint_value[5] = {0,90,90,0,0};
    static double new_joint_value[5];

    // 笛卡尔空间赋值
    new_decartes_point[0] = req.descartes_pos_commands[0];
    new_decartes_point[1] = req.descartes_pos_commands[1];
    new_decartes_point[2] = req.descartes_pos_commands[2];
    new_decartes_point[3] = req.descartes_pos_commands[3];
    new_decartes_point[4] = req.descartes_pos_commands[4];
    new_decartes_point[5] = req.descartes_pos_commands[5];

    // 当前关节角赋值 默认为[0,90,90,0,0] 即机械零点相对于运动学模型中绝对零点的角度值
    current_joint_value[0] = req.current_joint_state[0];
    current_joint_value[1] = req.current_joint_state[1];
    current_joint_value[2] = req.current_joint_state[2];
    current_joint_value[3] = req.current_joint_state[3];
    current_joint_value[4] = req.current_joint_state[4];

    // 调用逆运动学求解函数 
    if(!manipulator.IKine(new_decartes_point,current_joint_value,new_joint_value)){
        // 由于机器人是从机械零点开始运动，故需要将逆解所得的T2 T3关节角度减去90
        new_joint_value[1] -= 90;
        new_joint_value[2] -= 90;
        ROS_INFO_STREAM("ikine success");
        res.ifGetSolve=true;
        res.joint_pos_commands.clear();
        res.joint_pos_commands.push_back(new_joint_value[0]);
        res.joint_pos_commands.push_back(new_joint_value[1]);
        res.joint_pos_commands.push_back(new_joint_value[2]);
        res.joint_pos_commands.push_back(new_joint_value[3]);
        res.joint_pos_commands.push_back(new_joint_value[4]);
    }
    else{
        ROS_INFO_STREAM("ikine failed");
        res.ifGetSolve=false;
    }
    return true;
                     }

int main(int argc,char* argv[])
{
    ros::init(argc,argv,"manipulator_ik_server");
    ros::NodeHandle node_handle;
    ros::ServiceServer server = node_handle.advertiseService("inversekine",handle_function);
    
    ROS_INFO_STREAM("ik_server has been setted.");
    ros::spin();

    return 0;


}
